package de.rj.urwalking.util;

import com.google.android.gms.maps.model.LatLng;

import de.ralf.urwalking.soplugin.EPSGPoint;
import de.rj.urwalking.data.DoubleVector;

/**
 * Contains methods for converting between coordinate systems.
 * 
 * @author rjj
 */
public class GeoUtil {

	// Parametersatz zur Umrechnung
	private static final double dx = -597.095; // m
	private static final double dy = -71.406; // m
	private static final double dz = -412.097; // m
	private static final double ex = 0.894 * Math.PI / 180 / 3600; // winkelsekunden
	private static final double ey = 0.068 * Math.PI / 180 / 3600; // winkelsekunden
	private static final double ez = -1.563 * Math.PI / 180 / 3600; // winkelsekunden
	private static final double m = 0.99999242; // = -7.6 * Math.pow(10, -6) + 1;

	// Konstanten WGS84-Ellipsoid
	private static final double WGS84EllipsoidA = 6378137.000;
	private static final double WGS84EllipsoidB = 6356752.314;
	private static final double WGS84EllipsoidESquared = (Math.pow(WGS84EllipsoidA, 2) - Math.pow(WGS84EllipsoidB, 2))
			/ Math.pow(WGS84EllipsoidA, 2);

	// Konstanten Bessel-Ellipsoid
	private static final double besselEllipsoidA = 6377397.155;
	private static final double besselEllipsoidB = 6356078.962;
	private static final double besselEllipsoidESquared = (Math.pow(besselEllipsoidA, 2) - Math
			.pow(besselEllipsoidB, 2)) / Math.pow(besselEllipsoidA, 2);

	/**
	 * Convert latitude and longitude to EPSG:31468 coordinates. Algorithm extracted from excel file GPS_nach_GK.xls
	 * http://www.moenk.de/index.php?serendipity[subpage]=downloadmanager&level=1&thiscat=4
	 * 
	 * @param lon
	 *            longitude in degress
	 * @param lat
	 *            latitude in degrees
	 * 
	 * @return {@link EPSGPoint} containing easting and northing
	 */
	public static EPSGPoint convertLatLonToEPSG31468(double lon, double lat) {

		double N = WGS84EllipsoidA / Math.sqrt(1 - WGS84EllipsoidESquared * Math.pow(Math.sin(lat / 180 * Math.PI), 2));

		DoubleVector vector = new DoubleVector();
		vector.x = N * Math.cos(lat / 180 * Math.PI) * Math.cos(lon / 180 * Math.PI);
		vector.y = N * Math.cos(lat / 180 * Math.PI) * Math.sin(lon / 180 * Math.PI);
		vector.z = (N * Math.pow(WGS84EllipsoidB, 2) / Math.pow(WGS84EllipsoidA, 2)) * Math.sin(lat / 180 * Math.PI);

		// rotate
		double[][] matrix = new double[][] { { 1.0, ez, -ey }, { -ez, 1.0, ex }, { ey, -ex, 1 } };
		vector.matrixMult(matrix);

		// scale
		vector.scalarMul(m);

		// translate
		vector.add(dx, dy, dz);

		// vector is now in DHDN/Bessel
		// do some more scary math stuff:

		double s = Math.sqrt(Math.pow(vector.x, 2) + Math.pow(vector.y, 2));
		double T = Math.atan(vector.z * besselEllipsoidA / (s * besselEllipsoidB));
		double B = Math.atan((vector.z + besselEllipsoidESquared * Math.pow(besselEllipsoidA, 2) / besselEllipsoidB
				* Math.pow(Math.sin(T), 3))
				/ (s - besselEllipsoidESquared * besselEllipsoidA * Math.pow(Math.cos(T), 3)));

		double L = Math.atan(vector.y / vector.x);
		// double N2 = besselEllipsoidA / Math.sqrt(1 - besselEllipsoidESquared * Math.pow(Math.sin(B), 2));
		// double h = s / Math.cos(B) - N2;
		//
		// double BInDegrees = B * 180 / Math.PI;
		double LInDegrees = L * 180 / Math.PI;

		double L0 = 12.0;
		double l = (LInDegrees - L0) * Math.PI / 180;

		double eta = Math.sqrt(Math.pow(besselEllipsoidA, 2) / Math.pow(besselEllipsoidB, 2) * besselEllipsoidESquared
				* Math.pow(Math.cos(B), 2));
		double t = Math.tan(B);

		double n = (besselEllipsoidA - besselEllipsoidB) / (besselEllipsoidA + besselEllipsoidB);
		double alpha = (besselEllipsoidA + besselEllipsoidB) / 2
				* (1 + 0.25 * Math.pow(n, 2) + 0.015625 * Math.pow(n, 4));
		double beta = -3.0 / 2.0 * n + 9.0 / 16.0 * Math.pow(n, 3) - 3.0 / 32.0 * Math.pow(n, 5);
		double gamma = 0.9375 * Math.pow(n, 2) - 0.46875 * Math.pow(n, 4);
		double delta = -35.0 / 48.0 * Math.pow(n, 3) + 105.0 / 256.0 * Math.pow(n, 5);
		double epsilon = 315.0 / 512.0 * Math.pow(n, 4);

		double bogenlaenge = alpha
				* (B + beta * Math.sin(2 * B) + gamma * Math.sin(4 * B) + delta * Math.sin(6 * B) + epsilon
						* Math.sin(8 * B));

		double summandHochwert = t / 2 * N * Math.pow(Math.cos(B), 2) * Math.pow(l, 2);

		double hochwert = bogenlaenge + summandHochwert;

		double summandRechtswert1 = N * Math.cos(B) * l;
		double summandRechtswert2 = N / 6 * Math.pow(Math.cos(B), 3) * (1 - Math.pow(t, 2) + Math.pow(eta, 2))
				* Math.pow(l, 3);

		double rechtswert = summandRechtswert1 + summandRechtswert2 + 500000 + L0 / 3 * 1000000;

		return new EPSGPoint(rechtswert, hochwert);

	}

	/**
	 * Converts EPSG:31468 coordinates to latitude and longitude. Algorithm extracted from excel file GK_nach_GPS.xls
	 * http://www.moenk.de/index.php?serendipity[subpage]=downloadmanager&level=1&thiscat=4
	 * 
	 * @param northing
	 *            Hochwert (Y-Achse, Nord-Süd)
	 * @param easting
	 *            Rechtswert (X-Achse, Ost-West)
	 * @return {@link LatLng} containing latitude and longitude
	 */
	public static LatLng convertEPSG31468toLatLon(double northing, double easting) {
		double h = 0;

		double n = (besselEllipsoidA - besselEllipsoidB) / (besselEllipsoidA + besselEllipsoidB);
		double alpha = (besselEllipsoidA + besselEllipsoidB) / 2
				* (1 + 0.25 * Math.pow(n, 2) + 0.015625 * Math.pow(n, 4));
		double beta = 3.0 / 2.0 * n - 27.0 / 32.0 * Math.pow(n, 3) + 269.0 / 512.0 * Math.pow(n, 5);
		double gamma = 21.0 / 16.0 * Math.pow(n, 2) - 55.0 / 32.0 * Math.pow(n, 4);
		double delta = 151.0 / 96.0 * Math.pow(n, 3) - 417.0 / 128.0 * Math.pow(n, 5);
		double epsilon = 1097.0 / 512.0 * Math.pow(n, 4);

		double y0 = Math.floor(easting / 1000000);
		double L0 = y0 * 3;

		double y = easting - y0 * 1000000 - 500000;

		double B0 = northing / alpha; // 2;B10
		double Bf = B0 + beta * Math.sin(2 * B0) + gamma * Math.sin(4 * B0) + delta * Math.sin(6 * B0) + epsilon
				* Math.sin(8 * B0); // 2;11
		double Nf = besselEllipsoidA / Math.sqrt(1 - besselEllipsoidESquared * Math.pow(Math.sin(Bf), 2)); // 2;B13

		double nF = Math.sqrt(Math.pow(besselEllipsoidA, 2) / Math.pow(besselEllipsoidB, 2) * besselEllipsoidESquared
				* Math.pow(Math.cos(Bf), 2)); // 2;B14
		double tf = Math.tan(Bf); // 2;B15

		double BSummand1 = tf / 2 / Math.pow(Nf, 2) * (-1 - Math.pow(nF, 2)) * Math.pow(y, 2); // 2;17
		double BSummand2 = tf
				/ 24
				/ Math.pow(Nf, 4)
				* (5 + 3 * Math.pow(tf, 2) + 6 * Math.pow(nF, 2) - 6 * Math.pow(tf, 2) * Math.pow(nF, 2) - 4
						* Math.pow(nF, 4) - 9 * Math.pow(tf, 2) * Math.pow(nF, 4)) * Math.pow(y, 4); // 2;18
		// double BSummand3 = 0;
		// double BSummand4 = 0;

		double B = (Bf + BSummand1 + BSummand2) * 180 / Math.PI; // 2;B21

		double LSummand1 = 1 / Nf / Math.cos(Bf) * y; // 2;B23
		double LSummand2 = 1 / 6 / Math.pow(Nf, 3) / Math.cos(Bf)
				* (-1 - 2 * Math.pow(tf, 2) - Math.pow(nF, 2) * Math.pow(y, 3)); // 2;B24
		// double LSummand3 = 1
		// / 120
		// / Math.pow(Nf, 5)
		// / Math.cos(Bf)
		// * (5 + 28 * Math.pow(tf, 2) + 24 * Math.pow(tf, 4) + 6 * Math.pow(nF, 2) + 8 * Math.pow(tf, 2)
		// * Math.pow(nF, 2)) * Math.pow(y, 5); // 2;B25
		// double LSummand4 = 1 / 15040 / Math.pow(Nf, 7) / Math.cos(Bf)
		// * (-61 - 622 * Math.pow(tf, 2) - 1320 * Math.pow(tf, 4) - 720 * Math.pow(tf, 6)) * Math.pow(y, 7); // 2;B26

		double L = L0 + (LSummand1 + LSummand2) * 180 / Math.PI; // 2;B27

		double N = besselEllipsoidA / Math.sqrt(1 - besselEllipsoidESquared * Math.pow(Math.sin(B / 180 * Math.PI), 2)); // 3;B7

		DoubleVector vector = new DoubleVector();
		vector.x = (N + h) * Math.cos(B / 180 * Math.PI) * Math.cos(L / 180 * Math.PI); // 3;B9
		vector.y = (N + h) * Math.cos(B / 180 * Math.PI) * Math.sin(L / 180 * Math.PI); // 3;B10
		vector.z = (N * Math.pow(besselEllipsoidB, 2) / Math.pow(besselEllipsoidA, 2) + h)
				* Math.sin(B / 180 * Math.PI); // 3;B11

		double[][] matrix = new double[][] { { 1.0, -ez, ey }, { ez, 1.0, -ex }, { -ey, ex, 1 } };
		vector.matrixMult(matrix); // 4;A13-15

		vector.scalarMul(m); // 4;A18-20

		vector.add(-dx, -dy, -dz); // 4;A23-25

		double s = Math.sqrt(Math.pow(vector.x, 2) + Math.pow(vector.y, 2)); // 6;B7

		double T = Math.atan(vector.z * WGS84EllipsoidA / (s * WGS84EllipsoidB)); // 6;B8

		double B2 = Math.atan((vector.z + WGS84EllipsoidESquared * Math.pow(WGS84EllipsoidA, 2) / WGS84EllipsoidB
				* Math.pow(Math.sin(T), 3))
				/ (s - WGS84EllipsoidESquared * WGS84EllipsoidA * Math.pow(Math.cos(T), 3))); // 6;B9
		double L2 = Math.atan(vector.y / vector.x); // 6;B10

		B2 = B2 * 180 / Math.PI; // 6;B14
		L2 = L2 * 180 / Math.PI; // 6;B15

		return new LatLng(B2, L2);
	}
}
